perm filename X.SAI[LS,BGB] blob
sn#068836 filedate 1973-10-26 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00010 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 BEGIN "QBALL"
C00005 00003 α QUE-BALLS
C00007 00004 SUBR INIT
C00008 00005 REAL SUBR DOTVEC(ITG I,J) α DOT PERCEIVED CAMERA VECTORS
C00010 00006 SUBR LSCAM (ITG I,K,J) α LOCUS SOLVER
C00012 00007 SUBR MKROT
C00013 00008 SUBR PROJECT(REAL FOCAL) α SIMULATED CAMERA PROJECTION
C00014 00009 SUBR UNPROJECT(REAL FOCAL)
C00015 00010 α MAIN EXECUTION
C00032 ENDMK
C⊗;
BEGIN "QBALL"
REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
REQUIRE "SAITRG[SYS,BGB]" SOURCE_FILE;
REQUIRE "DPYIII[SYS,BGB]" SOURCE_FILE;
SAFE ITG ARRAY DPYBUF[0:1500];
α CAMERA;
REAL PAN,TILT,SWING; α CAMERA ORIENTATION;
REAL CX,CY,CZ; α CAMERA LOCATION;
REAL ARRAY CLX,CLY,CLZ[1:16];ITG CLI; α CAMERA SOLUTIONS;
REAL PDX,PDY,FOCAL; α PIXEL SIZE & LENS FOCAL LENGTH;
REAL ASPECT,MAG;
REAL RPA,CPA; α IMAGE LOCUS OF PRINCIPLE RAY;
REAL IX,IY,IZ;
REAL JX,JY,JZ;
REAL KX,KY,KZ;
α IRON TRIANGLE - CAMERA LOCUS SOLVER;
REAL ARRAY P1,P2,P3,COSANG[1:3],V[1:10,1:3];
REQUIRE "LS1V3P.REL" LOAD_MODULE;
EXTERNAL ITG PROCEDURE LS1V3P(REAL ARRAY V,P1,P2,P3,COSANG);
α QUE-BALLS;
REAL ARRAY XWC,YWC,ZWC[1:16]; α WORLD COORDINATES;
REAL ARRAY XCC,YCC,ZCC[1:16]; α PREDICTED CAMERA COORDINATES;
REAL ARRAY XPP,YPP,ZPP[1:16]; α PREDICTED IMAGE COORDINATES;
REAL ARRAY XDC,YDC,ZDC[1:16]; α DISPLAY COORDINATES;
REAL ARRAY PRROW,PRCOL[1:16]; α PERCEIVED ROW & COL;
REAL ARRAY PRXCC,PRYCC,PRZCC[1:16]; α PERCEIVED CAMERA COORDINATES;
REAL ARRAY PRXPP,PRYPP[1:16]; α PERCEIVED IMAGE COORDINATES;
REAL ARRAY PRXDC,PRYDC[1:16]; α PERCEIVED DISPLAY COORDINATES;
REAL C,S;ITG I;
SUBR INIT;
BEGIN "INIT"
α PRINCIPLE RAY LOCUS IN CAMERA'S IMAGE PLANE;
RPA ← 108; CPA ← 144;
α SIMULATED CAMERA LOCUS;
CX ← 10; CY ← -30; CZ ← 20;
α SIMULATED CAMERA ORIENTATION;
PAN ← 14*π/180; TILT ← 57*π/180; SWING ← 5*π/180;
MAG ← 32/9;
FOCAL ← 21000;
PDX ← 38.78; PDY ← 40.0;
END "INIT";
SUBR ORBINIT(REAL RADIUS); α INITIALIZE WORLD POINTS;
BEGIN "ORBINIT"
C ← COS(-π/8); S ← SIN(-π/8);
XWC[1] ← RADIUS;
YWC[1] ← ZWC[1] ← 0;
FOR I←2 THRU 16 DO
⊂ XWC[I] ← C*XWC[I-1] - S*YWC[I-1];
YWC[I] ← S*XWC[I-1] + C*YWC[I-1];
ZWC[I]←0; ⊃;
END "ORBINIT";
REAL SUBR DOTVEC(ITG I,J); α DOT PERCEIVED CAMERA VECTORS;
BEGIN "DOTVEC"
REAL X1,Y1,Z1,X2,Y2,Z2,R1,R2,ZCOS;
X1 ← PRXCC[I]; Y1 ← PRYCC[I]; Z1 ← PRZCC[I];
X2 ← PRXCC[J]; Y2 ← PRYCC[J]; Z2 ← PRZCC[J];
R1 ← SQRT(X1*X1 + Y1*Y1 + Z1*Z1);
R2 ← SQRT(X2*X2 + Y2*Y2 + Z2*Z2);
ZCOS←(X1*X2 + Y1*Y2 + Z1*Z2) / (R1*R2);
RETURN(ZCOS);
END "DOTVEC";
SUBR LSCAM (ITG I,K,J); α LOCUS SOLVER;
BEGIN "LSCAM"
ITG M,N,L;
REAL QMIN,Q;
α IRON TRIANGLE - KNOWN WORLD LOCI;
P1[1]←XWC[I]; P2[1]←XWC[J]; P3[1]←XWC[K];
P1[2]←YWC[I]; P2[2]←YWC[J]; P3[2]←YWC[K];
P1[3]←ZWC[I]; P2[3]←ZWC[J]; P3[3]←ZWC[K];
α IRON TRIPOD - KNOW ANGLES BETWEEN CAMERA RAYS;
COSANG[1] ← DOTVEC(J,K);
COSANG[2] ← DOTVEC(I,K);
COSANG[3] ← DOTVEC(I,J);
α THROW THE SHIT AT THE FAN;
M ← LS1V3P(V,P1,P2,P3,COSANG);
QMIN ← 999999;
FOR N←1 THRU M DO
BEGIN
Q ← SQRT ((V[N,1]-CX)↑2+(V[N,2]-CY)↑2+(V[N,3]-CZ)↑2);
IF Q<QMIN THEN ⊂ QMIN←Q;L←N;⊃;
END;
α OUTSTR(9&CVG(V[L,1])&9&CVG(V[L,2])&9&CVG(V[L,3])&↓);
α CAMERA LOCUS SOLUTIONS;
CLX[CLI]←V[L,1];
CLY[CLI]←V[L,2];
CLZ[CLI]←V[L,3];
CLI ← CLI+1;
END "LSCAM";
SUBR MKROT;
BEGIN "MKROT"
REAL RR;
REAL C_PAN,S_PAN,C_TILT,S_TILT,C_SWING,S_SWING;
C_PAN ← COS(PAN); S_PAN ← SIN(PAN);
C_TILT ← COS(TILT); S_TILT ← SIN(TILT);
C_SWING ← COS(SWING); S_SWING ← SIN(SWING);
IX ← C_PAN*C_SWING - S_PAN*C_TILT*S_SWING;
IY ← S_PAN*C_SWING + C_PAN*C_TILT*S_SWING;
IZ ← S_TILT*S_SWING;
JX ← -C_PAN*S_SWING - S_PAN*C_TILT*C_SWING;
JY ← -S_PAN*S_SWING + C_PAN*C_TILT*C_SWING;
JZ ← S_TILT*C_SWING;
KX ← S_PAN*S_TILT;
KY ← -C_PAN*S_TILT;
KZ ← C_TILT;
END "MKROT";
SUBR PROJECT(REAL FOCAL); α SIMULATED CAMERA PROJECTION;
BEGIN "PROJECT"
ITG I;
FOR I←1 THRU 16 DO
BEGIN
REAL X,Y,Z,SX,SY;
α WC → CC WORLD LOCII PREDICTED;
X ← XWC[I] - CX;
Y ← YWC[I] - CY;
Z ← ZWC[I] - CZ;
XCC[I] ← X*IX + Y*IY + Z*IZ;
YCC[I] ← X*JX + Y*JY + Z*JZ;
ZCC[I] ← X*KX + Y*KY + Z*KZ;
α CC → PP;
SX ← -FOCAL/PDX;
SY ← -FOCAL/PDY;
XPP[I] ← SX * XCC[I] / ZCC[I];
YPP[I] ← SY * YCC[I] / ZCC[I];
α PP → DC;
XDC[I] ← MAG * (XPP[I]+(CPA-144));
YDC[I] ← MAG * (YPP[I]+(RPA-108));
END;
END "PROJECT";
SUBR UNPROJECT(REAL FOCAL);
BEGIN "UNPROJECT"
ITG I;
FOR I←1 THRU 16 DO
BEGIN
PRXCC[I] ← XPP[I]*PDX;
PRYCC[I] ← YPP[I]*PDY;
PRZCC[I] ← -FOCAL;
END;
END "UNPROJECT";
α MAIN EXECUTION;
INIT;
ORBINIT(6);
MKROT;
PROJECT(FOCAL);
WHILE TRUE DO
BEGIN "MAIN"
ITG I;ITG ARRAY DPYBUF[1:1000];
REAL X0,Y0,Z0,X1,Y1,Z1;
REAL SDX,SDY,SDZ,DEL,DEL2,SD;
DPYSET(DPYBUF);
AIVECT(0,-500);AVECT(0,500);
AIVECT(500,-500);AVECT(-500,-500);
FOR DEL ← -15000 STEP 1000 UNTIL 15000 DO
BEGIN "LOOP"
UNPROJECT(FOCAL+DEL);CLI ← 1;
LSCAM(6,1,11);LSCAM(5,2,13);LSCAM(4,3,9);
LSCAM(10,4,16);LSCAM(9,8,3);LSCAM(14,10,6);
α AVERAGE SOLUTION;
X0←Y0←Z0←X1←Y1←Z1←0;
FOR I←1 THRU 6 DO
⊂ X0←X0+CLX[I];Y0←Y0+CLY[I];Z0←Z0+CLZ[I];
X1←X1+CLX[I]↑2;Y1←Y1+CLY[I]↑2;Z1←Z1+CLZ[I]↑2;⊃;
X0←X0/6; Y0←Y0/6; Z0←Z0/6;X1←X1/6; Y1←Y1/6; Z1←Z1/6;
α STANDARD DEVIATIONS;
SDX ← SQRT(X1 - X0↑2);SDY ← SQRT(Y1 - Y0↑2);SDZ ← SQRT(Z1 - Z0↑2);
SD ← SQRT(SDX↑2 + SDY↑2 + SDZ↑2);
AVECT(DEL*500/15000,50*SD-500);DPYOUT(1);
OUTSTR(9&CVS(DEL)&9&CVG(SD)&↓);
END "LOOP";
AVECT(500,-500);DPYOUT(1);AIVECT(-500,0);
INCHRW;
END "MAIN";
END "QBALL";